#ifndef MPATHNODE2D_H
#define MPATHNODE2D_H

#include <cmath>
namespace findPlanner {

class mNode2d {
 public:
  mNode2d(): mNode2d(0, 0, 0, 0, nullptr) {}
  mNode2d(int x, int y, float g, float h, mNode2d* pred) {
    this->x = x;
    this->y = y;
    this->g = g;
    this->h = h;
    this->pred = pred;
    this->o = false;
    this->c = false;
    this->idx = -1;
  }
  int getX() const { return x; }
  int getY() const { return y; }
  float getG() const { return g; }
  float getH() const { return h; }
  float getC() const { return g + h; }
  int getIdx() const { return idx; }
  bool  isOpen() const { return o; }
  bool  isClosed() const { return c; }
  mNode2d* getPred() const { return pred; }

  void setX(const int& x) { this->x = x; }
  void setY(const int& y) { this->y = y; }
  void setG(const float& g) { this->g = g; }
  void setH(const float& h) { this->h = h; }
  int setIdx(int width) { this->idx = y * width + x; return idx;}

  void open() { o = true; c = false; }
  void close() { c = true; o = false; }
  void reset() { c = false; o = false; g = 0; h = 0; pred = nullptr;}
  void setPred(mNode2d* pred) { this->pred = pred; }

  void updateG() { g += CostUnsymmetry(*pred); }
  void updateH(const mNode2d& goal) { h = movementCost(goal); }

  // 成本代价值
  float CostUnsymmetry(const mNode2d &pred) const { return ((x - pred.x) * (x - pred.x) + (y - pred.y) * (y - pred.y)); }

  // 欧式启发
  // float movementCost(const mNode2d &pred) const { return sqrt((x - pred.x) * (x - pred.x) + (y - pred.y) * (y - pred.y)) * 1.0001; }

  // 使用平方距离替代开方运算，以提高计算速度
  float movementCost(const mNode2d &pred) const { return ((x - pred.x) * (x - pred.x) + (y - pred.y) * (y - pred.y)) * 1.0001; }

  // 机器现在使用的启发值
  // distance = sqrt(pow((goal->idx_x - start->idx_x) * resolution_, 2) + pow((goal->idx_y - start->idx_y) * resolution_, 2));
  // return distance * 1.0001;  // tie breaker


  // 曼哈顿启发
  // float movementCost(const mNode2d &pred) const { return std::fmax(abs(x - pred.x), abs(y - pred.y)) * 1.0001; }

  // 打破对称性  对角线距离（切比雪夫距离）
  // float movementCost(const mNode2d &pred) const{
  //   float dx = abs(x - pred.x);
  //   float dy = abs(y - pred.y);
  //   return dx + dy + 1.414f * (std::fmin(dx, dy) - 0.5 * (dx + dy - std::fmin(dx, dy)));
  // }

  // 打破对称性 通过将直线距离减去 0.5 * (dx - dy) 的绝对值，这样做可以使得当 dx 和 dy 不完全相等时，对角线距离和直线距离之间存在微小的差异
  // float movementCost(const mNode2d &goal) const{
  //   int dx = abs(x - goal.x);
  //   int dy = abs(y - goal.y);
  //   return dx + dy + 1.414f * (std::fmin(dx, dy) - 0.5f * std::abs(dx - dy)); // 添加微小的不对称性
  // }

  bool operator == (const mNode2d& rhs) const;
  bool isOnGrid(const int width, const int height) const;

  mNode2d* createSuccessor(const int i);
  static const int dir;
  static const int dx[];
  static const int dy[];

 private:
  int x;
  int y;
  float g;
  float h;
  int idx;
  bool o;
  bool c;
  mNode2d* pred;
};


}
#endif // mNode2d_H
